/*
* @File Name trajectory.h
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\route\trajectory\trajectory.h
* @Author: Ruige_Lee
* @Date:   2019-09-05 20:31:52
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-09-09 21:21:09
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*/





#ifndef __TRAJECTORY_H__
#define __TRAJECTORY_H__

#include "stepper/steppers.h"
#include <math.h>


#define TRAJECTORY_MAX_PONITS 500


typedef struct
{
	double position[6];
	double timePoint;
	double ParaM[6];
	double tempU[6];
} Cspline_Point;


typedef struct
{
	int count;
	Cspline_Point points[TRAJECTORY_MAX_PONITS];
} Cspline_Line;



// extern float poseFIFO[STEPPER_FIFO_DP][6];
// extern uint16_t poseFIFO_read_prt;
// extern uint16_t poseFIFO_write_prt;
// extern uint8_t poseFIFO_full;
// extern uint8_t poseFIFO_empty;




extern float poseCheckArray[STEPPER_FIFO_DP][6];
extern uint32_t checkArrayPoint;
extern uint32_t checkArrayMax;



extern int16_t stepperFIFO[2][STEPPER_FIFO_DP][6];
extern uint16_t FIFO_read_prt[2];
extern uint16_t FIFO_write_prt[2];


extern Cspline_Line Cspline[2];

extern int32_t tra_checkIn(uint8_t robot);

extern int32_t tra_autoSet_timePoint( uint8_t robot );







#endif

